#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/exceptions.h>
#include <chrono>

using namespace std::chrono_literals;

class TFListener:public rclcpp::Node{
public:
    explicit TFListener(std::string name):Node(name){
        timer_=this->create_wall_timer(1s,std::bind(&TFListener::tf_listener_on_timer,this));
        // parameter
        // target_frame=this->declare_parameter("target_frame","house"); // statis tf
        target_frame=this->declare_parameter("target_frame","world"); // dynamic tf
        source_frame=this->declare_parameter("source_frame","turtle1");

        tf_buffer_=std::make_unique<tf2_ros::Buffer>(this->get_clock());
        tf_listener_=std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    }

    void tf_listener_on_timer(){
        geometry_msgs::msg::TransformStamped tf_info;

        try{
            tf_info=tf_buffer_->lookupTransform(target_frame,source_frame,tf2::TimePointZero);
        }catch(const tf2::TransformException & ex){
            RCLCPP_WARN(this->get_logger(),"Could not get tranform from %s to %s: %s",\
            source_frame.c_str(),target_frame.c_str(),ex.what());
            return;
        }

        tf2::Quaternion q(
            tf_info.transform.rotation.x,
            tf_info.transform.rotation.y,
            tf_info.transform.rotation.z,
            tf_info.transform.rotation.w);

        tf2::Matrix3x3 m(q);
        double roll,pitch,yaw;
        m.getRPY(roll,pitch,yaw);

        RCLCPP_INFO(this->get_logger(),"Get %s --> %s tf: [%f, %f, %f] [%f, %f, %f]",\
            target_frame.c_str(),
            source_frame.c_str(),
            tf_info.transform.translation.x,
            tf_info.transform.translation.y,
            tf_info.transform.translation.z,
            roll,pitch,yaw);
    }

private:
    // 1, create buffer obj
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_{nullptr};
    // 2, create tf listener obj
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
    rclcpp::TimerBase::SharedPtr timer_{nullptr};
    std::string source_frame{},target_frame{};
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TFListener>("tf_listener");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
  }